#include "./flash.h"
#include "../device/compat.h"

namespace driver::flash {

static constexpr uint32_t Key1 = 0x45670123;
static constexpr uint32_t Key2 = 0xCDEF89AB;

static void un_lock() {
    if ((FMC_CTL0 & FMC_CTL0_LK) == 0) {
        return;
    }
    FMC_KEY0 = Key1;
    FMC_KEY0 = Key2;
}

static void wait_busy() {
    for (; (FMC_STAT0 & FMC_STAT0_BUSY) != 0 ;);
}

static void page_erase(uint32_t addr) {
    un_lock();
    wait_busy();
    FMC_CTL0 |= FMC_CTL0_PER;
    FMC_ADDR0 = addr;
    FMC_CTL0 |= FMC_CTL0_START;
    wait_busy();
}

static void write_uint16(uint32_t addr, uint16_t val) {
    un_lock();
    wait_busy();
    FMC_CTL0 |= FMC_CTL0_PG;
    REG16(addr) = val;
    wait_busy();
}

void write(uint32_t addr, const void *buf, uint32_t size) {
    uint32_t index = 0;
    const uint16_t *p = reinterpret_cast<const uint16_t *>(buf);
    for (; index < size; index += 2) {
        write_uint16(addr + index, *p);
        p ++;
    }
}

void erase(uint32_t addr, uint32_t size) {
    uint32_t index = 0;
    for (; index < size; index += SECTOR_SIZE) {
        page_erase(addr + index);
    }
}

void move(uint32_t target, uint32_t src, uint32_t size) {
    erase(target, size);
    write(target, reinterpret_cast<const void *>(src), size);
}

void read(uint32_t addr, void *buf, uint32_t size) {
    uint8_t *p = reinterpret_cast<uint8_t *>(buf);
    for (uint32_t i = 0; i < size; i ++) {
        p[i] = REG8(addr + i);
    }
}

}
